/*
 * main.c
 *
 *  Created on: Oct 8, 2011
 *  Author: Mario H. Voorsluys
 *	email: forcaeluz@gmail.com
 *
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Library General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, visit:
 * http://www.gnu.org/copyleft/gpl.html
 */

#include <avr/interrupt.h>
#include <avr/io.h>
#include <util/delay.h>

#include "serial.h"
#include "analog_digital.h"
#include "cmd_err_src.h"
#include "twi_interface.h"

#define MYADDR 0x0001
unsigned char in_message[IN_SIZE]; // 	0xFF, 0xFF, COMMAND, 0x00, ADDR, ADDR, 0x00, OPTION1,    OPTION2,    0x00
unsigned char out_message[OUT_SIZE];// 	0xFF, 0xFF, SOURCE,  0x00, ADDR, ADDR, 0x00, VALUE(LSB), VALUE(MSB), 0x00
int mes_pos=0;

unsigned char b_output = 0xFE;
unsigned char enabled_adcs;

/*
 *
 */
void init(){
	DDRB = 0xFF;
	serial_init();
	init_adc();
	twi_init();
	sei();
}

void twi_error(){
	out_message[2] = CAO_ERROR;
	out_message[4] = TWI_ERROR;
	out_message[5] = TWI_ERROR>>8;
	out_message[7] = last_status;
	out_message[8] = 0x00;
}

void handle_message(){
	//Received address from the computer. Right now 1 byte is enought, but you never know.
	int address = in_message[4] + (in_message[5]<<8);
	//Declaring integer that stores the response.
	int response = 0;

	//Default error message. The necessary bytes will be changed if commands get executed.
	out_message[0] = 0xFF;//Header byte 1
	out_message[1] = 0xFF;//Header byte 2
	out_message[3] = 0x00;//Space byte
	out_message[6] = 0x00;//Space byte
	out_message[9] = 0x00;//Closure byte

	//Check if message is valid.
	if(in_message[0] != 0xFF || in_message[1] != 0xFF || in_message[3] != 0x00 || in_message[6] != 0x00 || in_message[9] != 0x00){
		out_message[2] = CAO_ERROR;								//There was a problem. This is the
		out_message[4] = (unsigned char)GENERAL_ERROR;		//Source Address 1 (LSB) (Error type)
		out_message[5] = (unsigned char)(GENERAL_ERROR>>8);	//Source Address 2 (MSB)
		out_message[7] = INVALID_INPUT_SEQUENCE;			//Error value (LSB)
		out_message[8] = INVALID_INPUT_SEQUENCE>>8;			//Error value (MSB)
		flush_serial();		//To avoid further problems, the buffer is flushed. Another option is to search for a new header, and start from there.
	}else{//If it is, let's do something
		switch(in_message[2]){
		case IDENTIFY_COMMAND:				//Case the command is an identify command
			out_message[2] = OWN_ID; 		//The data source is my own ID
			out_message[4] = MYADDR;		//Returns device id to the pc.
			out_message[5] = MYADDR>>8;
			out_message[7] = REVNUMBER; 		//Returns revision number to the pc. Nothing is really done with it right now.
			out_message[8] = (REVNUMBER>>8);
			break;
		case ADC_COMMAND: 					//Case the command is an ADC Reading.
			response = adc_value(address);	//Get response from the ADC. (Check datasheet for address).
			out_message[2] = ADC_SOURCE;	//Set source as ADC
			out_message[4] = address;		//Address same as asked
			out_message[5] = address>>8;
			out_message[7] = response;		//Set response into the two response bytes.
			out_message[8] = (response>>8); //
			break;

		case TWI_COMMAND:
			out_message[2] = TWI_SOURCE;	//Set source byte.
			out_message[4] = address;		//Address same as asked
			out_message[5] = address>>8;
			twi_send_start(address);		//Request Master
			if(last_status!=0x00){
				twi_error();
				break;
			}
			PORTB = 0x01;
			twi_master_tx_data(0x00);		//Send register address to write to.
			if(last_status!=0x00){			//Now this code is specific for the sonar commmands. (Register 0).
				twi_error();				//In the future this could be sent as option1 or option2 from the pc.
				break;
			}
			PORTB = 0x02;
			twi_master_tx_data(0x51);		//Send command (Range in cm)
			if(last_status!=0x00){
				twi_error();
				break;
			}
			twi_finish();
			PORTB = 0x03;//Free bus
			_delay_ms(70);		//Wait 70 ms, necessary for the sonar to range.
			PORTB = 0x04;
			out_message[7] = twi_request(address, 3); //Request the value from the sonar (LSB)
			PORTB = 0x05;
			if(last_status!=0x00){
				twi_error();
				break;
			}
			PORTB = 0x06;
			out_message[8] = twi_request(address, 2); //Request the value from the sonar (MSB)
			if(last_status!=0x00){
				twi_error();
				break;
			}
			break;
		default:
			out_message[2] = CAO_ERROR;	//Set source byte.
			out_message[4] = (unsigned char)GENERAL_ERROR;
			out_message[5] = (unsigned char)(GENERAL_ERROR>>8);
			out_message[7] = UNKOWN_COMMAND;
			out_message[8] = UNKOWN_COMMAND>>8;
			break;
		}
	}
}


/*
 *
 */
void loop(){
	while(mes_pos < 10){
		int input = serial_read();
		if(input == -1){
			return;
		}else{
			in_message[mes_pos]= input;
			mes_pos++;
		}
	}
	mes_pos = 0;
	handle_message();
	serial_writte(out_message, 10);
}

/*
 *
 */
int main(void) {
	init();
	while(1){
		loop();
	}
	return 0;
}
